﻿using System;
using System.Collections.Generic;
using System.Text;
using RobotRace.engine.agents;
using System.Threading;
using RobotRace.engine.raceengine.exceptions;
using RobotRace.engine.robossConnector;
using RoBOSSCommunicator;
using FIRADriverLibrary;
using Geometry;

namespace RobotRace.engine.raceengine
{
    public class RaceEngine
    {
        private List<Agent> agents;
        private Thread raceThread;
        private volatile bool shouldStop = false;
        private volatile bool shouldPause = false;
        private Object pauseMutex = new Object();
        private RobossConnector connector;
        public TimeSpan BestLap { get; private set; }

        public RaceEngine(List<Agent> agents, string ip, string port) : this(agents, RobossConnector.getConnector(ip, port)) { }

        public RaceEngine(List<Agent> agents, RobossConnector connector)
            : this(agents)
        {
            this.connector = connector;
        }

        private RaceEngine(List<Agent> agents)
        {
            this.agents = agents;
        }

        public void startRace()
        {
            configureAll();
            if (raceThread != null)
            {
                throw new IllegalRaceStateException("Race has already been started!");
            }
            raceThread = new Thread(new ThreadStart(this.run));
            raceThread.Start();
        }

        public void stopRace()
        {
            if (raceThread == null)
            {
                throw new IllegalRaceStateException("No race has been started - cannot stop!");
            }
            shouldStop = true;
            if (isPaused())
            {
                resumeRace();
            }
            raceThread.Join();
            raceThread = null;
            shouldStop = false;
        }

        public void pauseRace()
        {
            if (raceThread == null)
            {
                throw new IllegalRaceStateException("No race has been started - cannot pause!");
            }
            shouldPause = true;
        }

        public void resumeRace()
        {
            if (raceThread == null)
            {
                throw new IllegalRaceStateException("No race has been started - cannot resume!");
            }
            if (!isPaused())
            {
                throw new IllegalRaceStateException("No race has been paused - cannot resume!");
            }
            shouldPause = false;
            Monitor.Enter(pauseMutex);
            Monitor.PulseAll(pauseMutex);
            Monitor.Exit(pauseMutex);
        }

        public Boolean isPaused()
        {
            if (raceThread == null || shouldPause == false)
            {
                return false;
            }
            return true;
        }

        public Boolean isStopped()
        {
            return shouldStop || raceThread == null;
        }

        private void configureAll()
        {
            foreach (Agent agent in agents)
            {
                agent.configure(this, connector);
            }
        }

        private void run()
        {
            while (!shouldStop)
            {
                if (shouldPause)
                {
                    foreach (Agent agent in agents)
                    {
                        agent.pause();
                    }
                    connector.getFiraDriver().Step();
                    while (shouldPause)
                    {
                        Monitor.Enter(pauseMutex);
                        Monitor.Wait(pauseMutex);
                        Monitor.Exit(pauseMutex);
                    }
                }
                foreach (Agent agent in agents)
                {
                    agent.step();
                }
                connector.getFiraDriver().Step();
                Thread.Sleep(5);
            }
            stopAllAgents();
            
            connector.getFiraDriver().Step();
        }

        private void stopAllAgents()
        {
            foreach (Agent agent in agents)
            {
                agent.stop();
            }
        }

        public void registerLapTime(TimeSpan lapTime)
        {
            if (BestLap.TotalMilliseconds == 0.0)
            {
                BestLap = lapTime;
            }
            else if (BestLap.CompareTo(lapTime) > 0)
            {
                BestLap = lapTime;
            }
        }


        public void resetBestLap()
        {
            BestLap = new TimeSpan();
        }
    }
}
